#include "maindefs.h"
#include <i2c.h>
#include "my_i2c.h"
#include <string.h>

static i2c_comm *ic_ptr;

// Configure for I2C Master mode -- the variable "slave_addr" should be stored in
//   i2c_comm (as pointed to by ic_ptr) for later use.

void i2c_configure_master(unsigned char slave_addr)
{
	// Your code goes here
}

// Sending in I2C Master mode [slave write]
// 		returns -1 if the i2c bus is busy
// 		return 0 otherwise
// Will start the sending of an i2c message -- interrupt handler will take care of
//   completing the message send.  When the i2c message is sent (or the send has failed)
//   the interrupt handler will send an internal_message of type MSGT_MASTER_SEND_COMPLETE if
//   the send was successful and an internal_message of type MSGT_MASTER_SEND_FAILED if the
//   send failed (e.g., if the slave did not acknowledge).  Both of these internal_messages
//   will have a length of 0.
// The subroutine must copy the msg to be sent from the "msg" parameter below into
//   the structure to which ic_ptr points [there is already a suitable buffer there].

unsigned char i2c_master_send(unsigned char length, unsigned char *msg)
{
	// Your code goes here
}

// Receiving in I2C Master mode [slave read]
// 		returns -1 if the i2c bus is busy
// 		return 0 otherwise
// Will start the receiving of an i2c message -- interrupt handler will take care of
//   completing the i2c message receive.  When the receive is complete (or has failed)
//   the interrupt handler will send an internal_message of type MSGT_MASTER_RECV_COMPLETE if
//   the receive was successful and an internal_message of type MSGT_MASTER_RECV_FAILED if the
//   receive failed (e.g., if the slave did not acknowledge).  In the failure case
//   the internal_message will be of length 0.  In the successful case, the
//   internal_message will contain the message that was received [where the length
//   is determined by the parameter passed to i2c_master_recv()].
// The interrupt handler will be responsible for copying the message received into

unsigned char i2c_master_recv(unsigned char length)
{
	// Your code goes here
}


void start_i2c_slave_reply(signed char length, unsigned char *msg)
{
	if(length != MSGLEN) {
		HANDLE_ERROR(length);
		return;
	}

	for (ic_ptr->outbuflen = 0; ic_ptr->outbuflen < length; ic_ptr->outbuflen++) {
		ic_ptr->outbuffer[ic_ptr->outbuflen] = msg[ic_ptr->outbuflen];
	}
	ic_ptr->outbuflen = length;
	ic_ptr->outbufind = 1; // point to the second byte to be sent

	// put the first byte into the I2C peripheral
	SSPBUF = ic_ptr->outbuffer[0];
	// we must be ready to go at this point, because we'll be releasing the I2C
	// peripheral which will soon trigger an interrupt
	SSPCON1bits.CKP = 1;

}

// an internal subroutine used in the slave version of the i2c_int_handler

void handle_start(unsigned char data_read)
{
	ic_ptr->event_count = 1;
	ic_ptr->buflen = 0;
	// check to see if we also got the address	
	if (data_read) {
		if (SSPSTATbits.D_A == 1) {
			// this is bad because we got data and
			// we wanted an address
			ic_ptr->status = I2C_IDLE;
			ic_ptr->error_count++;
			ic_ptr->error_code = I2C_ERR_NOADDR;
		} else {
			if (SSPSTATbits.R_W == 1) {
				ic_ptr->status = I2C_SLAVE_SEND;
			} else {
				ic_ptr->status = I2C_RCV_DATA;
			}
		}
	} else {
		ic_ptr->status = I2C_STARTED;
	}
}

// this is the interrupt handler for i2c -- it is currently built for slave mode
// -- to add master mode, you should determine (at the top of the interrupt handler)
//    which mode you are in and call the appropriate subroutine.  The existing code
//    below should be moved into its own "i2c_slave_handler()" routine and the new
//    master code should be in a subroutine called "i2c_master_handler()"

void i2c_int_handler()
{
	unsigned char i2c_data = 0;
	unsigned char data_read = 0;
	unsigned char data_written = 0;
	unsigned char msg_ready = 0;
	unsigned char msg_to_send = 0;
	unsigned char overrun_error = 0;
	unsigned char error_buf[3];
	signed char retval = 0;
	signed char length = 0;
	unsigned char msgbuffer[MSGLEN + 1];
	//Temporary to setup message format
	static unsigned char msg_counter = 0;
	static unsigned char sensor_counter = 0;
	static unsigned char loco_counter = 0;
	signed char data_ind = 0;
	unsigned char i2c_msgtype = 0;
	unsigned char first = 0;
	unsigned char second = 0;

	// clear SSPOV
	if (SSPCON1bits.SSPOV == 1) {
		SSPCON1bits.SSPOV = 0;
		// we failed to read the buffer in time, so we know we
		// can't properly receive this message, just put us in the
		// a state where we are looking for a new message
		ic_ptr->status = I2C_IDLE;
		overrun_error = 1;
		ic_ptr->error_count++;
		ic_ptr->error_code = I2C_ERR_OVERRUN;
	}
	// read something if it is there
	if (SSPSTATbits.BF == 1) {
		i2c_data = SSPBUF;
		data_read = 1;
	}

#if !SENSORS_PIC
	// toggle an LED
	I2CLED = !I2CLED;
#endif

	if (!overrun_error) {
		switch (ic_ptr->status) {
		case I2C_IDLE:
		{
			// ignore anything except a start
			if (SSPSTATbits.S == 1) {
				handle_start(data_read);
				// if we see a slave read, then we need to handle it here
				if (ic_ptr->status == I2C_SLAVE_SEND) {
					data_read = 0;
					msg_to_send = 1;
				}
			}
			break;
		}
		case I2C_STARTED:
		{
			// in this case, we expect either an address or a stop bit
			if (SSPSTATbits.P == 1) {
				// we need to check to see if we also read an
				// address (a message of length 0)
				ic_ptr->event_count++;
				if (data_read) {
					if (SSPSTATbits.D_A == 0) {
						msg_ready = 1;
					} else {
						ic_ptr->error_count++;
						ic_ptr->error_code = I2C_ERR_NODATA;
					}
				}
				ic_ptr->status = I2C_IDLE;
			} else if (data_read) {
				ic_ptr->event_count++;
				if (SSPSTATbits.D_A == 0) {
					if (SSPSTATbits.R_W == 0) { // slave write
						ic_ptr->status = I2C_RCV_DATA;
					} else { // slave read
						ic_ptr->status = I2C_SLAVE_SEND;
						msg_to_send = 1;
						// don't let the clock stretching bit be let go
						data_read = 0;
					}
				} else {
					ic_ptr->error_count++;
					ic_ptr->status = I2C_IDLE;
					ic_ptr->error_code = I2C_ERR_NODATA;
				}
			}
			break;
		}
		case I2C_SLAVE_SEND:
		{
			if (ic_ptr->outbufind < ic_ptr->outbuflen) {
				SSPBUF = ic_ptr->outbuffer[ic_ptr->outbufind];
				ic_ptr->outbufind++;
				data_written = 1;
			} else {
				// we have nothing left to send
				ic_ptr->status = I2C_IDLE;
			}
			break;
		}
		case I2C_RCV_DATA:
		{
			// we expect either data or a stop bit or a (if a restart, an addr)
			if (SSPSTATbits.P == 1) {
				// we received a stop bit
				// we need to check to see if we also read data
				ic_ptr->event_count++;
				if (data_read) {
					if (SSPSTATbits.D_A == 1) {
						// we received data
						ic_ptr->buffer[ic_ptr->buflen] = i2c_data;
						ic_ptr->buflen++;
						msg_ready = 1;
					} else {
						// we received an address
						ic_ptr->error_count++;
						ic_ptr->error_code = I2C_ERR_NODATA;
						ic_ptr->status = I2C_IDLE;
					}
				} else {
					msg_ready = 1;
				}
				ic_ptr->status = I2C_IDLE;
			} else if (data_read) {
				ic_ptr->event_count++;
				if (SSPSTATbits.D_A == 1) {
					// we received data, store it in the buffer
					ic_ptr->buffer[ic_ptr->buflen] = i2c_data;
					ic_ptr->buflen++;
				} else /* a restart */ {
					if (SSPSTATbits.R_W == 1) {
						ic_ptr->status = I2C_SLAVE_SEND;
						msg_ready = 1;
						msg_to_send = 1;
						// don't let the clock stretching bit be let go
						data_read = 0;
					} else { /* bad to recv an address again, we aren't ready */
						ic_ptr->error_count++;
						ic_ptr->error_code = I2C_ERR_NODATA;
						ic_ptr->status = I2C_IDLE;
					}
				}
			}
			break;
		}
		}
	}

	// release the clock stretching bit (if we should)
	if (data_read || data_written) {
		// release the clock
		if (SSPCON1bits.CKP == 0) {
			SSPCON1bits.CKP = 1;
		}
	}

	// must check if the message is too long, if 
	if ((ic_ptr->buflen > MAXI2CBUF - 2) && (!msg_ready)) {
		ic_ptr->status = I2C_IDLE;
		ic_ptr->error_count++;
		ic_ptr->error_code = I2C_ERR_MSGTOOLONG;
	}

	if (msg_ready) {
		// We have received the full write message from the master
		ic_ptr->buffer[ic_ptr->buflen] = ic_ptr->event_count;
		ic_ptr->buflen = 0;
	} else if (ic_ptr->error_count >= I2C_ERR_THRESHOLD) {
		error_buf[0] = ic_ptr->error_count;
		error_buf[1] = ic_ptr->error_code;
		error_buf[2] = ic_ptr->event_count;
		retval = ToMainHigh_sendmsg(sizeof(unsigned char) *3, MSGT_I2C_DBG, (void *) error_buf);
		ic_ptr->error_count = 0;
	}
	if (retval < 0) {
		// if an error from sendmsg, call error handler
		HANDLE_ERROR(retval);
	}
	if (msg_to_send) {
		// send the data out
		unsigned char reg_recvd = ic_ptr->buffer[0]; // can skip this step
		unsigned char msgtype = 0;
		// Get the data sent from main over the queue
		length = FromMainHigh_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
		if (length <= 0) {
			// If we didn't receive something from the queue, send this
			memset(msgbuffer, -1, MSGLEN);
			i2c_msgtype = I2C_MSGT_NO_NEWDATA;
			length = -1;
		}
		else {
#if SENSORS_PIC
			i2c_msgtype = I2C_MSGT_SENSORS;
			if(sensor_counter != msgbuffer[1]) {
				// Msg lost somewhere?
				//HANDLE_ERROR(sensor_counter - msgbuffer[0]); // temp error value
			}
			sensor_counter++;
			// start an A/D conversion to begin reading the sensors again
			ADCON0bits.GO_DONE = 1;
#elif LOCONET_PIC
//			LATAbits.LATA0 = 1;
			i2c_msgtype = I2C_MSGT_LOCO;
			if(loco_counter != msgbuffer[1]) {
				// Msg lost somewhere? 
				//HANDLE_ERROR(ERR_MSG_LOST); // temp error value
			}
			loco_counter++;
#endif
			// Fill the rest of msgbuffer with -1, easier to see on logic analyzer
			memset(msgbuffer + (sizeof(unsigned char) * length), -1, MSGLEN-length);

			// Shift the data over two bytes
			if(length > MSGLEN - 2) {
				// ERROR msg too large, discarding the rest
				length = 14;
			}
			for (data_ind = length - 1; data_ind >= 0; data_ind--) {
				msgbuffer[data_ind + 2] = msgbuffer[data_ind];
			}
		}
		

		// Place type/length in first byte. nibble 1 = type. nibble 2 = length of data
		first = (i2c_msgtype & 0x0F) | (length << 4);
		msgbuffer[0] = first;

		// Place counter in second byte.
		//TODO setup the msg_counter when the message is first created
		second = msg_counter++;
		msgbuffer[1] = second;

		start_i2c_slave_reply(MSGLEN, msgbuffer);
#if LOCONET_PIC
//		LATAbits.LATA0 = 0;
#endif
	}
	msg_to_send = 0;
}

// set up the data structures for this i2c code
// should be called once before any i2c routines are called

void init_i2c(i2c_comm *ic)
{
	ic_ptr = ic;
	ic_ptr->buflen = 0;
	ic_ptr->event_count = 0;
	ic_ptr->status = I2C_IDLE;
	ic_ptr->error_count = 0;
}

// setup the PIC to operate as a slave
// the address must include the R/W bit

void i2c_configure_slave(unsigned char addr)
{

	// ensure the two lines are set for input (we are a slave)
	TRISCbits.TRISC3 = 1; // SCL1
	TRISCbits.TRISC4 = 1; // SDA1
	// set the address
	SSPADD = addr;
	//OpenI2C(SLAVE_7,SLEW_OFF); // replaced w/ code below
	//Clear the status and control registers
	SSPSTAT = 0x0;
	SSPCON1 = 0x0;
	SSPCON2 = 0x0;
	// Set to I2C Slave mode, 7-bit address w/ Start/Stop interrupts
	SSPCON1 |= 0x0E;
	// Disable slew rate control for standard speed control
	SSPSTAT |= SLEW_OFF;
	// Set clock and data high to be set for input, although they already should be
	I2C_SCL = 1;
	I2C_SDA = 1;
	// enable clock-stretching 
	SSPCON2bits.SEN = 1;
	SSPCON1 |= SSPENB;
	// end of i2c configure
}
